#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "ares_main/rot_msgs.h"
#include "ares_main/sonar_msgs.h"
#include "ares_main/rot_LS_msg.h"
#include <cmath>
#include <sstream>
#include <time.h>

#define SONARRNG 5
#define PI 3.141592653589793238462643383
#define ROTCUTOFF 12

static ros::Subscriber sub_vis_odom, sub_rot, sub_sonar, sub_LS;
static ros::Publisher pub_cmd;

double vel_in, omega_in, spd1, spd2, spd3, spd4; 
double wheel_sep;
double sonarData [4][5] = { 0 };
double UpFm, DnFm, UpBm, DnBm;
bool spinlock = false;

//factors to multiply by spd# in order to prevent or encourage wheel module rotation
double fct1, fct2, fct3, fct4;

double LSangle;

int i, j;

int sonarUpFi, sonarDnFi, sonarUpBi, sonarDnBi;
int sonarUpFm, sonarDnFm, sonarUpBm, sonarDnBm;

void backup();

double angleMag(double angle) {
    if ((int(angle*180/PI) % 90 >= 68) || (int(angle*180/PI) % 90 <= 22)){
        return 1;
    } else {
        return 1.41;
    }
}

int nextAngleSignDiff(double dAngle1, double dAngle2) {
    if ((dAngle1 == 0) || (dAngle2 == 0)) {
        return 0;
    } else if (int(dAngle1/fabs(dAngle1)) != int(dAngle2/fabs(dAngle2))) {
        return 1;
    } else {
        return 0;
    }
}

double distanceWeighting(double d) {
    //more or less arbitrary equation with which to weight the deltarads by distance from current
    //y=(x-3)^2-2x+7 : (0, 16) and (4, 0) so normalize magnitude by 16, and make anything beyond 4, 0.
    
    if (d < 4) {
        return ((d-3)*(d-3)-2*d+7)/10;
    } else {
        return 0;
    }
}

void callback_vis_odom(geometry_msgs::Twist const &twist)
{
    vel_in = twist.linear.x;
    omega_in = fabs(omega-LSangle)*twist.angular.z;
}

void callback_LS(ares_main::rot_LS_msg const &LS_msg)
{
    LSangle = LS_msg.rotLS*PI/180;
}

void callback_rot(ares_main::rot_msgs const &rot_msg)
{
    fct1 = rot_msg.rot1/150 + 1;
    fct2 = rot_msg.rot2/150 + 1;
    fct3 = rot_msg.rot3/150 + 1;
    fct4 = rot_msg.rot4/150 + 1;
}

void callback_sonar(ares_main::sonar_msgs const &sonar_msg)
{
    //    v                                    v         v    : v vv
    //EX: mean value of sonar that is pointing Up in the front "UpFm"
    
    UpFm = 0;
    DnFm = 0;
    UpBm = 0;
    DnBm = 0;
    
    for (i=0;i<5;i++) {
        if (i <= sonarUpFm) {UpFm += sonarData[0][i];}
        if (i <= sonarDnFm) {DnFm += sonarData[1][i];}
        if (i <= sonarUpBm) {UpBm += sonarData[2][i];}
        if (i <= sonarDnBm) {DnBm += sonarData[3][i];}
    }
    
    UpFm = UpFm/(sonarUpFm+1);
    DnFm = DnFm/(sonarDnFm+1);
    UpBm = UpBm/(sonarUpBm+1);
    DnBm = DnBm/(sonarDnBm+1);
    
    if ((sonar_msg.sonarUpFront <= UpFm+SONARRNG)
        && (sonar_msg.sonarUpFront >= UpFm-SONARRNG)) {
        sonarUpFi += 1;
        if (sonarUpFi >= 5) {sonarUpFi = 0;}
        sonarData[0][sonarUpFi] = sonar_msg.sonarUpFront;
        if ((sonarUpFi-1 == sonarUpFm) && (sonarUpFm <= 4)) {
            sonarUpFm += 1;
        }
    } else {
        sonarData[0][0] = sonar_msg.sonarUpFront;
        sonarUpFi = 0;
        sonarUpFm = 0;
    }
    
    if ((sonar_msg.sonarDownFront <= DnFm+SONARRNG)
        && (sonar_msg.sonarDownFront >= DnFm-SONARRNG)) {
        sonarDnFi += 1;
        if (sonarDnFi >= 5) {sonarDnFi = 0;}
        sonarData[1][sonarDnFi] = sonar_msg.sonarDownFront;
        if ((sonarDnFi-1 == sonarDnFm) && (sonarDnFm <= 4)) {
            sonarDnFm += 1;
        }
    } else {
        sonarData[1][0] = sonar_msg.sonarDownFront;
        sonarDnFi = 0;
        sonarDnFm = 0;
    }
    
    if ((sonar_msg.sonarUpBack <= UpBm+SONARRNG)
        && (sonar_msg.sonarUpBack >= UpBm-SONARRNG)) {
        sonarUpBi += 1;
        if (sonarUpBi >= 5) {sonarUpBi = 0;}
        sonarData[2][sonarUpBi] = sonar_msg.sonarUpBack;
        if ((sonarUpBi-1 == sonarUpBm) && (sonarUpBm <= 4)) {
            sonarUpBm += 1;
        }
    } else {
        sonarData[2][0] = sonar_msg.sonarUpBack;
        sonarUpBi = 0;
        sonarUpBm = 0;
    }
    
    if ((sonar_msg.sonarDownBack <= DnBm+SONARRNG)
        && (sonar_msg.sonarDownBack >= DnBm-SONARRNG)) {
        sonarDnBi += 1;
        if (sonarDnBi >= 5) {sonarDnBi = 0;}
        sonarData[3][sonarDnBi] = sonar_msg.sonarDownBack;
        if ((sonarDnBi-1 == sonarDnBm) && (sonarDnBm <= 4)) {
            sonarDnBm += 1;
        }
    } else {
        sonarData[3][0] = sonar_msg.sonarDownBack;
        sonarDnBi = 0;
        sonarDnBm = 0;
    }
    
    //ROS_INFO("max indeces: UpFm = %d, DnFm = %d, UpBm = %d, DnBm = %d", sonarUpFm, sonarDnFm, sonarUpBm, sonarDnBm);
    //ROS_INFO("enter indeces: UpFm = %d, DnFm = %d, UpBm = %d, DnBm = %d", sonarUpFi, sonarDnFi, sonarUpBi, sonarDnBi);
    
    if ((UpFm < 30) || (DnFm > 110)) {
        backup();
    }
}

void mSleep(int mS) {
    struct timespec tim, tim2;
    tim.tv_sec = 0;
    tim.tv_nsec = mS*1000000L;
    nanosleep(&tim , &tim2);
}

void backup() {
    spinlock = true;
    drive_commands::drive_raw msg;
    for (i = 0;i <= 3000;i+=10) {
        msg.spd1 = -0.7 + 0.5 * wheel_sep * omega_in;
        msg.spd2 = -0.7 - 0.5 * wheel_sep * omega_in;
        msg.spd3 = -0.7;
        msg.spd4 = -0.7;
        pub_cmd.publish(msg);
        mSleep(10);
    }
    spinlock = false;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "drive_cmds");
    ros::NodeHandle nh;
    
    sub_vis_odom = nh.subscribe("path/cmd_vel", 1, &callback_vis_odom);
    sub_LS = nh.subscribe("LS_data", 10, &callback_LS);
    sub_rot = nh.subscribe("rotation_data", 10, &callback_rot);
    sub_sonar = nh.subscribe("sonar_data", 10, &callback_sonar);
    
    pub_cmd = nh.advertise<drive_commands::drive_raw>("drive/cmd_vel_raw", 10);
    
    ros::param::get("~wheel_separation", wheel_sep);
    
    ros::Rate r(10);
    
    sonarUpFi = 0;
    sonarDnFi = 0;
    sonarUpBi = 0;
    sonarDnBi = 0;
    
    sonarUpFm = 0;
    sonarDnFm = 0;
    sonarUpBm = 0;
    sonarDnBm = 0;
    
    //sonar data array order:
    //Front up, Front down, Back up, Back down
    //[which sonar][which recorded number]
    
    while ((nh.ok()) && !(spinlock)) {
        
        //ROS_INFO("UpFm = %f, DnFm = %f, UpBm = %f, DnBm = %f", UpFm, DnFm, UpBm, DnBm);
        
        spd1 = vel_in - 0.5 * wheel_sep * omega_in;
        spd2 = vel_in + 0.5 * wheel_sep * omega_in;
        spd3 = vel_in;
        spd4 = vel_in;
        
        drive_commands::drive_raw msg;
  
        msg.spd1 = fct1*spd1;
        msg.spd2 = fct2*spd2;
        msg.spd3 = fct3*spd3;
        msg.spd4 = fct4*spd4;
  
        pub_cmd.publish(msg);
        
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}
